#pragma once

#include "../LidarDriverX.h"
#include "UdpReceiver.h"

struct LidarPoint
{
    short x{0};
    short y{0};
    short z{0};
    unsigned short Distance{0}; // 距离 = Distance × Dis Unit
    std::byte Reflectivity{0};  // 反射率 = Reflectivity × 1%   范围：0 ~ 255
    // double Confidence;   // 低置信度标记，即回波信号的可信程度。    取值：1（标记低置信度），0（正常）
};

class LidarDriverHS : public LidarDriverX
{
public:
    LidarDriverHS(LidarConfigurationOptions *options = new LidarConfigurationOptions(), int id = 0);

private:
    template <size_t N>
    int16_t ToInt16(const std::array<std::byte, N> bytes, size_t startIndex);
    template <size_t N>
    int32_t ToInt32(const std::array<std::byte, N> bytes, size_t startIndex);
    template <size_t N>
    uint32_t ToUInt32(const std::array<std::byte, N> bytes, size_t startIndex);

private:
    std::string _correction_file_path;
    uint16_t _data_port;
    std::string _remote_ip, _local_ip;
    bool _is_raw_data = false;

    UdpReceiver *_udpReceiver = nullptr;

    std::vector<std::tuple<double, double>> lstFramesBound;
    std::vector<double> lstAzimuth_Offset;
    std::vector<double> lstElevation;

    robosense::lidar::SyncQueue<std::shared_ptr<PointCloudData<pcl::PointXYZI>>> _free_cloud_queue;
    robosense::lidar::SyncQueue<std::shared_ptr<PointCloudData<pcl::PointXYZI>>> _stuffed_cloud_queue;

    int lastFrameIndex{-1};
    int currentFrameIndex{-1};
    uint32_t _frame_seq = 0;

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud0;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2;

    std::shared_ptr<PointCloudData<pcl::PointXYZI>> getPointCloud();
    void setPointCloud(std::shared_ptr<PointCloudData<pcl::PointXYZI>> data); 

    bool ReadAngleCorrectionFile(std::string path, std::vector<std::tuple<double, double>> &lstFramesBound, std::vector<double> &lstAzimuth_Offset, std::vector<double> &lstElevation);
    void OnDatagramReceived(const std::vector<uint8_t> &data);
    bool SetPoints(std::array<std::byte, 512> bytes, double DisUnit, double azimuth, std::vector<std::tuple<double, double>> lstFramesBound, std::vector<double> lstAzimuth_Offset, std::vector<double> lstElevation);

protected:
    void init() override;
    void dispose() override;
    void processPointCloud() override;
};